from spatialmath.base import *

#rokae pose express  Rotz(C)Roty(B)Rotx(A)

# //check
def rokae_pose_to_T(rokae_pose):
    x = rokae_pose[0]
    y = rokae_pose[1]
    z = rokae_pose[2]
    A = rokae_pose[3]
    B = rokae_pose[4]
    C = rokae_pose[5]
    T = transl(x, y, z) @ rpy2tr(A, B, C, order='zyx', unit='rad')
    return T

def T_to_rokae_pose(T):
    ret_list = [0,0,0,0,0,0]
    ret_list[0] = T[0, 3]
    ret_list[1] = T[1, 3]
    ret_list[2] = T[2, 3]
    rpy = tr2rpy(T, unit="rad", order="zyx", check=False)
    ret_list[3] = rpy[0]
    ret_list[4] = rpy[1]
    ret_list[5] = rpy[2]
    return ret_list

def halcon_pose_to_T(halcon_pose, type='abg'):
    x = halcon_pose[0]
    y = halcon_pose[1]
    z = halcon_pose[2]
    rx = halcon_pose[3]
    ry = halcon_pose[4]
    rz = halcon_pose[5]
    type_int = halcon_pose[6]

    if type_int == 0: # RxRyRz
        T = transl(x, y, z) @ rpy2tr(rz, ry, rx, order='xyz', unit='deg')
        return T
    elif type_int == 2: #RzRyRx
        T = transl(x, y, z) @ rpy2tr(rx, ry, rz, order='zyx', unit='deg')
        return T

    if type == 'gba': # RxRyRz
        T = transl(x, y, z) @ rpy2tr(rz, ry, rx, order='xyz', unit='deg')
        return T
    elif type=='abg': #RzRyRx
        T = transl(x, y, z) @ rpy2tr(rx, ry, rz, order='zyx', unit='deg')
        return T
    assert True, ValueError("格式错误")

def T_to_halcon_pose(T, type_int = 0, type='abg'):
    def angle_to_360(angle_list):
        for i in range(3,6):
            if angle_list[i] < 0.0:
                angle_list[i] = angle_list[i] + 360
        return angle_list

    ret_list = [0,0,0,0,0,0]
    ret_list[0] = T[0, 3]
    ret_list[1] = T[1, 3]
    ret_list[2] = T[2, 3]

    if type_int == 0: # RxRyRz
        rpy = tr2rpy(T, unit="deg", order="xyz", check=False)
        ret_list[5] = rpy[0]
        ret_list[4] = rpy[1]
        ret_list[3] = rpy[2]
        return angle_to_360(ret_list)
    elif type_int == 2: #RzRyRx
        rpy = tr2rpy(T, unit="deg", order="zyx", check=False)
        ret_list[3] = rpy[0]
        ret_list[4] = rpy[1]
        ret_list[5] = rpy[2]
        return angle_to_360(ret_list)

    if type == 'gba': # RxRyRz
        rpy = tr2rpy(T, unit="deg", order="xyz", check=False)
        ret_list[5] = rpy[0]
        ret_list[4] = rpy[1]
        ret_list[3] = rpy[2]
        return angle_to_360(ret_list)
    elif type=='abg': #RzRyRx
        rpy = tr2rpy(T, unit="deg", order="zyx", check=False)
        ret_list[3] = rpy[0]
        ret_list[4] = rpy[1]
        ret_list[5] = rpy[2]
        return angle_to_360(ret_list)

    assert True, ValueError("格式错误")


def xyzrpy_to_T(x,y,z,roll,pitch,yaw):
    T = transl(x, y, z) @ rpy2tr(roll, pitch, yaw, order='zyx', unit='deg')
    return T

def T_to_xyzrpy(T):
    ret_list = [0, 0, 0, 0, 0, 0]
    ret_list[0] = T[0, 3]
    ret_list[1] = T[1, 3]
    ret_list[2] = T[2, 3]
    rpy = tr2rpy(T, unit="deg", order="zyx", check=False)
    ret_list[3] = rpy[0]
    ret_list[4] = rpy[1]
    ret_list[5] = rpy[2]
    return ret_list
# 'abg'
# trinv()
if __name__ == '__main__':
    angle = [-0.128686, -0.0199921, -0.0670786, 91.1342, 357.553, 358.194, 2]
    T = halcon_pose_to_T(angle)
    halcon_pose = T_to_halcon_pose(T,2)
    print("OVER")

